#include "Selection.h"
#include "stdio.h"

static float Direction[20];
void Selection_init(){
    //准双向口 P00 P01
    P0_MODE_IO_PU(GPIO_Pin_0 | GPIO_Pin_1);
}

void Selection_scan(){
    char res;
	int i;
	float distance;

    for(i = 0;i<=180;i+=10){
		set_angle_value(i);
		res = Ultrasonic_get_distance(&distance);
		    printf("res = %d\n", (int)res);
		if (res == 0) {
		    printf("i = %d\n",i);
		    printf("distance = %.2f\n", distance);
		Direction[i/10] = distance;
		    printf("Direction[%d] = %.2f",i/10,Direction[i/10]);
	    }else{
	    	Direction[i/10] = -1.0;
	    }
		os_wait2(K_TMO, 20);
	}

    set_angle_value(90);
    os_wait2(K_TMO, 20);
}

MotorsMode Selection_turn(){
    float L_num = 0;
    float R_num = 0;
    u8 L_count = 0;
    u8 R_count = 0;
    int i;
    
    // 遍历右边方向
    for(i = 0; i < 9; i++) {
        // 如果当前方向距离大于最大距离且大于安全距离
         if(Direction[i] > SAFE_DISTANCE){
         	// L_num += Direction[i];
         	L_count++;
         }
    }
    // 遍历左边方向
    for(i = 10; i <= 18; i++) {
        // 如果当前方向距离大于最大距离且大于安全距离
        if(Direction[i] > SAFE_DISTANCE){
            // R_num += Direction[i];
            R_count++;
        }
    }

    // 比较左右方向的距离，选择最优方向
    // if(L_count > 0 && R_count > 0){
    //     if(L_num/L_count > R_num/R_count){
    //         return LEFT_M;
    //     }else
    //         return RIGHT_M;
    // }else if(L_count > 0){
    //     return LEFT_M;
    // }else if(R_count > 0){
    //     return RIGHT_M;
    // }
    // else    // 如果没有找到安全距离的方向，返回MID_M
    //     return MID_M;//返回MID_M则说明周围全是障碍物，需要调用Motors_backward(50,MID_M)先后退

    //根据左右方向的数量选择最优方向
    if(L_count > R_count){
        return LEFT_M;
    }else if(L_count < R_count){
        return RIGHT_M;
    }else{ // 如果左右方向的数量相等，返回MID_M}
        return MID_M;}
}

// -使用倒数计算风险值，距离越近风险越高,更准确反映障碍物距离的影响
// - 增加数据有效性检查
// - 对风险值进行标准化处理
// - 处理风险值相同的情况
// - 考虑了所有有效数据而非简单计数
static MotorsMode last = MID_M;
static int turn_value = 45;//转向时间，通过特定时间对应转动的特定角度，例如速度25转动90*5ms就是转动90°
MotorsMode Selection_turn_new(){
    float left_risk = 0.0f;
    float right_risk = 0.0f;
    int valid_left = 0;
    int valid_right = 0;
    u8 i;
    u8 max_index;
    
    // 计算左侧风险值 (0-9对应左侧90度范围)
    for(i = 0; i <= 9; i++) {
        if(Direction[i] > 0) { // 有效距离数据
            right_risk += 100.0f / Direction[i]; // 距离越近风险值越大
            valid_right++;
        }
    }
    
    // 计算右侧风险值 (10-18对应右侧90度范围)
    for(i = 9; i <= 18; i++) {
        if(Direction[i] > 0) { // 有效距离数据
            left_risk += 100.0f / Direction[i]; // 距离越近风险值越大
            valid_left++;
        }
    }
    
    // 标准化风险值
    if(valid_left > 0) left_risk /= valid_left;
        printf("left_risk = %.2f\n", left_risk);
    if(valid_right > 0) right_risk /= valid_right;
        printf("right_risk = %.2f\n", right_risk);
    
    // 比较左右侧风险值
    if(valid_left == 0 && valid_right == 0) {
        printf("MID_M\n");
        return MID_M; // 无有效数据，停止
    } else if(left_risk < right_risk) {
        max_index = 0;
        printf("LEFT_M\n");
        //返回数组最大值的下标
        for(i = 0; i < 9; i++) {
            if(Direction[i] > Direction[max_index]) {
                max_index = i;
            }
        }
        turn_value = 90 - (max_index * 10);
        last = LEFT_M;
        return LEFT_M; // 左侧风险较低
    } else if(left_risk > right_risk) {
        max_index = 10;
        printf("RIGHT_M\n");
        //返回数组最大值的下标
        for(i = 10; i <= 18; i++) {
            if(Direction[i] > Direction[max_index]) {
                max_index = i;
            }
        }
        turn_value = max_index * 10 - 90;
        last = RIGHT_M;
        return RIGHT_M; // 右侧风险较低
    } else {
        printf("last");
        turn_value = 45;//重置回默认值
        return  last;// 风险相同返回上一次的方向 
    }
}

void Selection_test(){
    while(1){
    	Motors_stop();
		os_wait2(K_TMO, 20);
        // Motors_backward(10,MID_M);
        // os_wait2(K_TMO,10);
        Selection_scan();
        if(Selection_turn_new() == MID_M){
            Motors_backward(50,MID_M);
            os_wait2(K_TMO,20);
            break;
        }
        Motors_around(50,Selection_turn_new());
        os_wait2(K_TMO, 20);
    }
}

void Selection_run(){
    //Motors_forward(50,Selection_turn());
    while(L == 0 && R != 0){
        Motors_translate(25,RIGHT_M);
        os_wait2(K_TMO, 20);
    }
    
    while(L != 0 && R == 0){
        Motors_translate(25,LEFT_M);
        os_wait2(K_TMO, 20);
    }

    while(L == 0 && R == 0){
	    Motors_stop();
		os_wait2(K_TMO, 20);
        // Motors_backward(10,MID_M);
        // os_wait2(K_TMO,10);
        Selection_scan();
        if(Selection_turn_new() == MID_M){
            Motors_backward(50,MID_M);
            os_wait2(K_TMO,20);
            break;
        }
        Motors_around(25,Selection_turn_new());
        os_wait2(K_TMO, turn_value);
    }
    
    Motors_forward(25,MID_M);
}